/*===========================================
Pew Pew Tanks Prototype

(C) Michael Michniewski 2010
============================================*/
#include <iostream>

#include <grrlib.h>
#include <stdlib.h>
#include <stdarg.h>
#include <wiiuse/wpad.h>
#include <wiiuse/wiiuse.h>

#include <debug.h>
#include "gecko_debug.h"

#include "map.h"

#include "gfx/BMfont1.h"

// RGBA Colors
#define GRRLIB_BLACK   0x000000FF
#define GRRLIB_MAROON  0x800000FF
#define GRRLIB_GREEN   0x008000FF
#define GRRLIB_OLIVE   0x808000FF
#define GRRLIB_NAVY    0x000080FF
#define GRRLIB_PURPLE  0x800080FF
#define GRRLIB_TEAL    0x008080FF
#define GRRLIB_GRAY    0x808080FF
#define GRRLIB_SILVER  0xC0C0C0FF
#define GRRLIB_RED     0xFF0000FF
#define GRRLIB_LIME    0x00FF00FF
#define GRRLIB_YELLOW  0xFFFF00FF
#define GRRLIB_BLUE    0x0000FFFF
#define GRRLIB_FUCHSIA 0xFF00FFFF
#define GRRLIB_AQUA    0x00FFFFFF
#define GRRLIB_WHITE   0xFFFFFFFF

/****************************************************************************
 * Main
 ***************************************************************************/

int main(int argc, char **argv) {

	USBGeckoOutput(); // uncomment to enable USB gecko output
	//__exception_setreload(8);
	
	#ifdef DEBUG_USB_GECKO
	DEBUG_Init(GDBSTUB_DEVICE_USB, 1);	// init debugging
	_break();
	#endif

    // Initialise the Graphics & Video subsystem
    GRRLIB_Init();

    // Initialise the Wiimotes
    WPAD_Init();
	// set IR sensor
	WPAD_SetDataFormat(WPAD_CHAN_0, WPAD_FMT_BTNS_ACC_IR);
	WPAD_SetVRes(0, 640, 480);
	
	// TEST:
	GRRLIB_ClipDrawing(0,0,rmode->fbWidth,rmode->efbHeight);
	
	// LOAD FONT
	GRRLIB_texImg *tex_BMfont1 = GRRLIB_LoadTexture(BMfont1);
    GRRLIB_InitTileSet(tex_BMfont1, 32, 32, 32);
	
	// Load background image
	/*
	const char *fileTexBG = "bg.";	// jpg, bmp, or png
	GRRLIB_texImg *texBG;
	texBG = GRRLIB_LoadTextureFromFile (fileTexBG);
	*/
	
	// todo: assert textures not null?
	
	// Load tank image
	const char *fileTexTank = "sd:/tank1.png";	// jpg, bmp, or png
	GRRLIB_texImg *texTank;
	texTank = GRRLIB_LoadTextureFromFile (fileTexTank);
	if (texTank == NULL) {
		debug(ERROR, "Error reading %s.\n", fileTexTank);
		exit(0);
	}
	GRRLIB_InitTileSet(texTank, 64, 64, 0);
	GRRLIB_SetMidHandle(texTank, true);		// rotate about center
	
	// load tank Turret
	const char *fileTexTurret = "sd:/turret1.png";	// jpg, bmp, or png
	GRRLIB_texImg *texTurret;
	texTurret = GRRLIB_LoadTextureFromFile (fileTexTurret);
	if (texTurret == NULL) {
		debug(ERROR, "Error reading %s.\n", fileTexTurret);
		exit(0);
	}
	GRRLIB_InitTileSet(texTurret, 24, 40, 0);
	GRRLIB_SetHandle(texTurret, 12, 24);		// set turret center for rotation
	
	// initialize tank position
	f32 tank_x, tank_y, tank_rot, turret_rot;
	tank_x = 320;
	tank_y = 240;
	tank_rot = 0;
	turret_rot = 0;

	// controller input data structures
	ir_t wpadIR0;
	u32 pressed, held;
	struct expansion_t exp;
	
	debug(OUT, "Testing stdout.\n");	// DEBUG
	
	// Load map
	Map map1;
	map1.Load((char*)"sd:/map_one.tmx");
	
    // Loop forever
    while(1) {
		// Scan the Wiimotes
		WPAD_ScanPads();
		pressed = WPAD_ButtonsDown(0);
		held = WPAD_ButtonsHeld(0);

        // If [HOME] was pressed on the first Wiimote, break out of the loop
        if (pressed & WPAD_BUTTON_HOME)  break;
		
		// RESET Tank to center
		if (pressed & WPAD_BUTTON_A) {
			tank_x = 320;
			tank_y = 240;
			tank_rot = 0;
			turret_rot = 0;
		}
		
		// NUNCHUK Tank Movement
		WPAD_Expansion(0, &exp);
		if (exp.type == EXP_NUNCHUK) {
			float ang = exp.nunchuk.js.ang;
			float mag = exp.nunchuk.js.mag;
			float angrad =  M_PI * ang / 180;
			// DEBUG: note, clear screen before drawing.
			//GRRLIB_Printf(5, 250, tex_BMfont1, GRRLIB_BLUE, 1, "M %f A %f", mag, ang);
			//GRRLIB_Printf(5, 320, tex_BMfont1, GRRLIB_BLUE, 1, "C %f S %f", mag * cos(angrad), mag * sin(angrad));
			
			if (mag > 0.10) {	// threshold
				f32 x = sin(angrad);	// reverse due to different stick angle origins
				f32 y = cos(angrad);
				tank_x +=  4 * mag * x;	// linear factor
				tank_y -=  4 * mag * y;
				tank_rot = atan(x/y) / M_PI * 180;	// convert angle to degrees from calculated x, y
			}
		}
		
		// IR Turret Movement
		WPAD_IR(WPAD_CHAN_0, &wpadIR0);
		if (wpadIR0.smooth_valid) {
			f32 x = wpadIR0.sx - tank_x;	// get x, y from IR wrt Tank
			f32 y = wpadIR0.sy - tank_y;
			turret_rot = atan(y/x) / M_PI * 180;	// calculate angle
			turret_rot = (x > 0) ? turret_rot + 90 : turret_rot - 90;		// fix off-by-90 degrees AND arctan phase of 180 degrees (we need to turn by 360)
			//GRRLIB_Printf(5, 60, tex_BMfont1, GRRLIB_BLUE, 1, "X %f R %f", x, turret_rot);
		}
		
		// BOUND Tank movement
		if (tank_x < 0) tank_x = 0;
		else if (tank_x > 600) tank_x = 600;
		if (tank_y < 0) tank_y = 0;
		else if (tank_y > 448) tank_y = 448;
		
        // ---------------------------------------------------------------------
        // Place your drawing code here
        // ---------------------------------------------------------------------
		
		// Clear the Screen (white)
		GRRLIB_FillScreen(RGBA(0xFF,0xFF,0xFF,0xFF));
		
		// draw background
		//GRRLIB_DrawImg(0, 0, texBG, 0, 1.0, 1.0, 0, 0);
		
		// draw Tank
		GRRLIB_DrawTile(tank_x, tank_y, texTank, tank_rot, 1.0, 1.0, GRRLIB_WHITE, 0);
		
		// draw Turret
		// draw on tank
		GRRLIB_DrawTile(tank_x-12, tank_y-24, texTurret, turret_rot, 1.0, 1.0, GRRLIB_WHITE, 0);
		
		// draw IR dot
		GRRLIB_Circle (wpadIR0.sx, wpadIR0.sy, 4, GRRLIB_RED, false);
		

        GRRLIB_Render();  // Render the frame buffer to the TV
    }

    GRRLIB_Exit(); // Be a good boy, clear the memory allocated by GRRLIB
	
	GRRLIB_FreeTexture(texTank);
	GRRLIB_FreeTexture(texTurret);
	//GRRLIB_FreeTexture(texBG);

    exit(0);  // Use exit() to exit a program, do not use 'return' from main()
}
